/**************************************************************
*
* M3D_MATR.H
*
* 2D/3D Matrix & Coordinate systems (CS) operations
**************************************************************/


//
// 2D Matrix
//
class m2dMATR {
public:
   union {
      float  elements[9];
      struct {
         float  _11, _12, _13;
         float  _21, _22, _23;
         float  _31, _32, _33;
      };
      float  m[3][3];
   };

   m2dMATR (void) { }
   m2dMATR (float _m00, float _m01, float _m02,
            float _m10, float _m11, float _m12,
            float _m20, float _m21, float _m22)
   {
      m[0][0] = _m00; m[0][1] = _m01; m[0][2] = _m02; 
      m[1][0] = _m10; m[1][1] = _m11; m[1][2] = _m12; 
      m[2][0] = _m20; m[2][1] = _m21; m[2][2] = _m22; 
   }

   float&  Element  (int n)        { return  elements[n];       }
   float&  ElementRC(int r, int c) { return  elements[r*3 + c]; }

   void  Identity   (void);
   void  Scale      (float x, float y, int operOrder);
   void  Rotate     (float angle, int operOrder);
   void  Translate  (float x, float y, int operOrder);
   void  Transform  (m2dMATR *pMTrans, int operOrder);
   BOOL  Invert     (m2dMATR *destMatr);
   void  GetOrigin  (m2dV *org);
   float CalcDet    (void);

   void  MakeRot        (float phi);
   void  MakeLSegLSeg   (m2dV *lB1, m2dV *lE1, m2dV *lB2, m2dV *lE2);
   BOOL  MakePolyPoly   (m2dPOLY *pPolyFrom, m2dPOLY *pPolyTo);


   __INLINE void  TransformPoint  (m2dV *p, m2dV *pResult) const;
   __INLINE void  TransformPoint  (m2dV *p) const;
   __INLINE void  TransformVector (m2dV *p, m2dV *pResult) const;
   __INLINE void  TransformVector (m2dV *p) const;
   __INLINE void  TransformPoly   (m2dPOLY *poly, m2dPOLY *polyResult) const;
};
#define m2dMatrElement(pMat,n)      ((pMat)->elements[(n)])
#define m2dMatrElementRC(pMat,r,c)  ((pMat)->elements[(r)*3 + (c)])

//
// 3D Matrix
//
class m3dMATR {
public:
   union {
      float  elements[16];
      struct {
         float  _11, _12, _13, _14;
         float  _21, _22, _23, _24;
         float  _31, _32, _33, _34;
         float  _41, _42, _43, _44;
      };
      float  m[4][4];
      m4dV  v[4];
   };

   m3dMATR (void) { }
   m3dMATR (float _m00, float _m01, float _m02, float _m03,
            float _m10, float _m11, float _m12, float _m13,
            float _m20, float _m21, float _m22, float _m23,
            float _m30, float _m31, float _m32, float _m33)
   {
      m[0][0] = _m00; m[0][1] = _m01; m[0][2] = _m02; m[0][3] = _m03;
      m[1][0] = _m10; m[1][1] = _m11; m[1][2] = _m12; m[1][3] = _m13;
      m[2][0] = _m20; m[2][1] = _m21; m[2][2] = _m22; m[2][3] = _m23;
      m[3][0] = _m30; m[3][1] = _m31; m[3][2] = _m32; m[3][3] = _m33;
   }


   float&        Element  (int n)              { return  elements[n];       }
   const float&  Element  (int n)        const { return  elements[n];       }
   float&        ElementRC(int r, int c)       { return  elements[r*4 + c]; }
   const float&  ElementRC(int r, int c) const { return  elements[r*4 + c]; }

   // Note on PC the compiler generated version will be faster
#if (defined _AP_XENON || (defined _AP_PS3 && !defined __SNC__)) && !defined M3D_SUPPRESS_OPTIMIZATION
   m3dMATR& operator= (const m3dMATR& other)
   {
	   const UINT64 * __RESTRICT pother = (UINT64*)other.elements;
	   UINT64 * __RESTRICT me = (UINT64*)this->elements;

	   me[0]	= pother[0];
	   me[1]	= pother[1];
	   me[2]	= pother[2];
	   me[3]	= pother[3];
	   me[4]	= pother[4];
	   me[5]	= pother[5];
	   me[6]	= pother[6];
	   me[7]	= pother[7];

      return *this;
   }
#endif

   // Operations, useful for 3D transformations
   void  Identity  (void);
   void  Translate (float x, float y, float z, int operOrder);
   void  Translate (const m3dV *v, int operOrder);
//   void  Rotate    (float x, float y, float z, float angle, int operOrder);
   void  RotateX   (float angle, int operOrder = M3D_POST);
   void  RotateY   (float angle, int operOrder = M3D_POST);
   void  RotateZ   (float angle, int operOrder = M3D_POST);
   void  Scale     (float x, float y, float z, int operOrder);
   void  Transform (const m3dMATR *pMTrans, int operOrder);
   BOOL  Invert    (m3dMATR *destMatr) const;
   void  Transpose (void);
   static void Transpose(const m3dMATR* pSrc, m3dMATR* pDst);

   // Get matrix origin
   void        GetOrigin    (m3dV * __RESTRICT org) const
   {
	   org->x = ElementRC(3,0);
	   org->y = ElementRC(3,1);
	   org->z = ElementRC(3,2);
	   m3dCheckValid(org);
   }

   m3dV*       GetOrigin    (void) const { return (m3dV*)&elements[12]; }
   void        GetAxisX     (m3dV * __RESTRICT vX) const
   {
	   vX->x = ElementRC(0,0);
	   vX->y = ElementRC(0,1);
	   vX->z = ElementRC(0,2);
	   m3dCheckValid(vX);
   }
   m3dV*       GetAxisX     (void) const { return (m3dV*)&elements[0];  }
   void        GetAxisY     (m3dV * __RESTRICT vY) const
   {
	   vY->x = ElementRC(1,0);
	   vY->y = ElementRC(1,1);
	   vY->z = ElementRC(1,2);
	   m3dCheckValid(vY);
   }
   m3dV*       GetAxisY     (void) const { return (m3dV*)&elements[4];  }
   void        GetAxisZ     (m3dV * __RESTRICT vZ) const
   {
	   vZ->x = ElementRC(2,0);
	   vZ->y = ElementRC(2,1);
	   vZ->z = ElementRC(2,2);
	   m3dCheckValid(vZ);
   }
   m3dV*       GetAxisZ     (void) const { return (m3dV*)&elements[8];  }

   // Misc 2D & 3D matrix operations
   void  GetTranslate (float * __RESTRICT x, float * __RESTRICT y, float * __RESTRICT z) const
   {
	   *x = ElementRC(3, 0);
	   *y = ElementRC(3, 1);
	   *z = ElementRC(3, 2);
   }
   void  GetTranslate (m3dV * __RESTRICT tran) const
   {
	   tran->x = ElementRC(3, 0);
	   tran->y = ElementRC(3, 1);
	   tran->z = ElementRC(3, 2);
   }
   void  GetRotateRot (float *rotX, float *rotY, float *rotZ) const;
   void  GetRotate    (float *rotX, float *rotY, float *rotZ) const;
   void  GetScale     (float *x, float *y, float *z) const;
   void  GetScaleX    (float *x) const;
   void  GetInvScale  (float *x, float *y, float *z) const;
   bool  IsNotUnitScale (void);
   bool  IsIdentity  (void) const;

   //
   // Special matrix creation
   //
   void  MakeTranslate  (float  x, float  y, float  z);
   void  MakeScale      (float sx, float sy, float sz);

   // make rotation matrices
   void  MakeRotX       (float angle);
   void  MakeRotY       (float angle);
   void  MakeRotZ       (float angle);
   void  MakeRot        (const m3dV *norm1, const m3dV *norm2);
   void  MakeRot        (const m3dV *norm1, const m3dV *norm2, float angle);
   void  MakeRot        (const m3dV *org, const m3dV *norm1, const m3dV *norm2);
   void  MakeRotRay     (m3dV const *org, m3dV const *dir, float angle);


   // Coordinate system mapping matrices
   void  MakeWCS2LCS    (const m3dV *org, const m3dV *vx, const m3dV *vy, const m3dV *vz);
   void _MakeWCS2LCS_VX (m3dV *org, m3dV *dir);
   void _MakeWCS2LCS_VY (m3dV *org, m3dV *dir);
   void _MakeWCS2LCS_VZ (m3dV *org, m3dV *dir);
   void  MakeLCS2WCS    (const m3dV *org, const m3dV *vx, const m3dV *vy, const m3dV *vz);
   void  MakeLCS2WCS_VZ (m3dV *org, m3dV *dir);
   void _MakeLCS2WCS_VX (m3dV *org, m3dV *dir); 
   void _MakeLCS2WCS_VY (m3dV *org, m3dV *dir); 
   void _MakeLCS2WCS_VZ (m3dV *org, m3dV *dir); 
   BOOL  MakeTRS        (float tX, float tY, float tZ, float rX, float rY, float rZ, float sX, float sY, float sZ);
   void  Orthonormalize (void);
   // Advanced transformation matrices
   void  MakeProjTri    (m3dV *v1, m3dV *v2, m3dV *v3);
   BOOL  MakePolyPoly   (m3dPOLY *pPolyFrom, m3dPOLY *pPolyTo);
   BOOL  MakeTriTri     (m3dTRI  *pTriFrom,  m3dTRI  *pTriTo);


   void  MakeLookAt         (const m3dV *pEye, const m3dV *pAt, const m3dV *pUp);    // Build a lookat matrix. (left-handed)
   void  MakeViewForwUp     (const m3dV *pEye, const m3dV *pForw, const m3dV *pUp);  // Build a view matrix. (left-handed)
   void  MakePersp          (float w, float h, float zn, float zf);                  // Build a perspective projection matrix. (left-handed)
   void  MakePerspFOV       (float fovy, float Aspect, float zn, float zf);          // Build a perspective projection matrix. (left-handed)
   void  MakePerspOffCenter (float l, float r, float b, float t, float zn,float zf); // Build a perspective projection matrix. (left-handed)
   void  MakeOrtho          (float w, float h, float zn, float zf);                  // Build an ortho projection matrix. (left-handed)
   void  MakeOrthoOffCenter (float l, float r, float b, float t, float zn,float zf); // Build an ortho projection matrix. (left-handed)

   void  RemoveScale    (void);

   float CalcDet        (void) const;
   float CalcNorm       (void) const;
   float CalcCondValue  (void) const;

   // Aux matrix functions
   bool  IsRightHanded  (void) const;
   BOOL  Check          (void) const;
   void  CheckOrtho     (void) const;

   //
   // Vertex/Polygon transformations
   //
   void  __CDECL TransformPointList  (int nPoint,                      // number of points to transform
                                      void *srcPoints, int srcStride,  // srcStride is a size of srcPoints array element
                                      void *dstPoints, int dstStride) const;   // dst == src is OK
   void  __CDECL TransformVectorList (int nVector,                     // number of points to transform
                                      void *srcPoints, int srcStride,  // srcStride is a size of srcPoints array element
                                      void *dstPoints, int dstStride) const;   // dst == src is OK
   __INLINE void  __CDECL TransformPoint  (m3dV const *p, m3dV *pResult) const;
   __INLINE void  __CDECL TransformPoint  (m3dV *p) const;
   __INLINE void  __CDECL TransformPoint  (m3dV const *p, m4dV *pResult) const;
   __INLINE void  __CDECL TransformPointHomogeneous  (m4dV const *p, m4dV *pResult) const;
   
   __INLINE void  __CDECL ReverseTransformPoint (m3dV const *p, m3dV *pResult) const;         

   __INLINE void  __CDECL TransformVector (const m3dV *p, m3dV *pResult) const;
   __INLINE void  __CDECL TransformVector (m3dV *p) const;

   __INLINE void  TransformNormal (const m3dV *pNorm, m3dV *pRes) const;
   __INLINE void  TransformNormal (m3dV *pNorm) const;
   
   __INLINE void  TransformPoly   (m3dPOLY *poly3D, m3dPOLY *poly3DResult) const;
   __INLINE void  TransformPoly   (m3dPOLY *poly3D) const;

   void  MakeEuler      (float angle_x, float angle_y, float angle_z);

   //
   // m3dMATRd support
   //
   m3dMATR(const class m3dMATRd& v)
   {
      *this = v;
   }
   inline m3dMATR& operator= (const class m3dMATRd& v);

private:
   float _CalcMatrDet (short row1, short row2, short row3, short col1, short col2, short col3) const;
};
#define m3dMatrElement(pMat,n)      ((pMat)->elements[(n)])
#define m3dMatrElementRC(pMat,r,c)  ((pMat)->elements[(r)*4 + (c)])

class m3dMATR34 {
public:
   void           FromMatr       (const m3dMATR & m) {
      STRONG_ASSERT(m3dIsZero(m.v[3].x) && m3dIsZero(m.v[3].y) && m3dIsZero(m.v[3].z) && m3dIsEqual(m.v[3].w, 1.f));
      memcpy(this, &m, 3 * 4 * sizeof(float));
   }

   void           ToMatr         (m3dMATR & m) {
      memcpy(&m, this, 3 * 4 * sizeof(float));
      m.v[3].x = m.v[3].y = m.v[3].z = 0;
      m.v[3].w = 1;
   }

private:
   float data[3 * 4];
};


//
// 3D Matrix (double)
//
class m3dMATRd {
public:
   union {
      double  elements[16];
      struct {
         double  _11, _12, _13, _14;
         double  _21, _22, _23, _24;
         double  _31, _32, _33, _34;
         double  _41, _42, _43, _44;
      };
      double  m[4][4];
   };

   m3dMATRd(void) {};

   m3dMATRd(const class m3dMATR& v)
   {
      *this = v;
   }

   m3dMATRd& operator= (const m3dMATR& v) 
   {
      for (int i = 0; i < 4; ++i)
         for (int j = 0; j < 4; ++j)
            m[i][j] = v.m[i][j];

      return *this;
   }

   double&        ElementRC(int r, int c)       { return  elements[r*4 + c]; }
   const double&  ElementRC(int r, int c) const { return  elements[r*4 + c]; }

   BOOL  Invert  (m3dMATRd *destMatr) const;

   void Identity (void)
   {
      m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = 0.0;
      m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; m[1][3] = 0.0;
      m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; m[2][3] = 0.0;
      m[3][0] = 0.0; m[3][1] = 0.0; m[3][2] = 0.0; m[3][3] = 1.0;
   }

   void Transpose(void);
   static void Transpose(const m3dMATRd* pSrc, m3dMATRd* pDst);
};


inline m3dMATR& m3dMATR::operator= (const m3dMATRd& v)
{
   for (int i = 0; i < 4; ++i)
      for (int j = 0; j < 4; ++j)
         m[i][j] = (float)v.m[i][j];

   return *this;
}


void m3dMultiplyMatrAccuracy(const m3dMATRd *a, const m3dMATRd *b, m3dMATRd *c);


//
// Coordinate system
//
class m3dCS {
public:
   m3dMATR    matrL2W;     // matrix LCS->WCS
   m3dMATR    matrW2L;     // matrix WCS->LCS
};



//
// Functions
//
extern BOOL m3dInit2DMatr (void);
extern void m3dTerm2DMatr (void);

extern BOOL m3dInit3DMatr (void);
extern void m3dTerm3DMatr (void);


//
// Simple matrix manipulation functions (C = A oper B)
// 
extern void m3dAddMatr       (m3dMATR const *a, m3dMATR const *b, m3dMATR *c);
extern void m3dSubtractMatr  (m3dMATR const *a, m3dMATR const *b, m3dMATR *c);
extern void m3dMultiplyMatr  (m3dMATR const *a, m3dMATR const *b, m3dMATR *c);
extern void m3dMultiplyMatrAccuracy(const m3dMATR *a, const m3dMATR *b, m3dMATR *c);
extern void m3dDivideMatr    (m3dMATR const *a, m3dMATR const *b, m3dMATR *c);
extern void m3dDivideMatrLeft(m3dMATR const *a, m3dMATR const *b, m3dMATR *c);

extern void m2dAddMatr       (m2dMATR const *a, m2dMATR const *b, m2dMATR *c);
extern void m2dSubtractMatr  (m2dMATR const *a, m2dMATR const *b, m2dMATR *c);
extern void m2dMultiplyMatr  (m2dMATR const *a, m2dMATR const *b, m2dMATR *c);


//
// Comparing 3D-matrices
//
extern BOOL m3dIsEqualMatr_A (const m3dMATR *pMat1, const m3dMATR *pMat2, float acc_rot, float acc_pos);
#define m3dIsEqualMatr(pM1,pM2)  m3dIsEqualMatr_A(pM1,pM2,M3D_EPSILON,M3D_EPSILON)

//
// 3D Transformations
//
/**************************************************************
* m3dMATR::TransformPoint()
*
**************************************************************/
__INLINE void __CDECL m3dMATR::TransformPoint (m3dV const*p, m3dV * __RESTRICT pResult) const
{
   m3dCheckValid(p);
#if defined _AP_XENON && !defined M3D_SUPPRESS_OPTIMIZATION
   {
      XMMATRIX mtx    = XMLoadFloat4x4((const XMFLOAT4X4*)this->elements);
      XMVECTOR v      = XMLoadVector3(p);
      XMVECTOR result = XMVector3Transform(v, mtx);
      XMStoreVector3(pResult, result);

	  return;
   }
#elif defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   {
	   vec_float4 m0 = vec_load_unaligned(this->elements, 0);
	   vec_float4 m1 = vec_load_unaligned(this->elements, 16);
	   vec_float4 m2 = vec_load_unaligned(this->elements, 32);
	   vec_float4 m3 = vec_load_unaligned(this->elements, 48);
	   vec_float4 v = vec_load_unaligned((float*)p);
	   vec_float4 xxxx = vec_splat(v, 0);
	   vec_float4 yyyy = vec_splat(v, 1);
	   vec_float4 zzzz = vec_splat(v, 2);

	   vec_float4 result = vec_madd(zzzz, m2, m3);
	   result = vec_madd(yyyy, m1, result);
	   result = vec_madd(xxxx, m0, result);

	   vec_store3_unaligned(result, (float*)pResult);
   }
#else
   // FPU version - note that this is 1.5x-2x faster than the old SSE asm version.
   // SSE would be faster if m3dMATR and m3dV were 16-byte aligned, m3dV was widened to 4 floats
   // (to store in 1 op without overwriting the next float value), and/or if transforms were done
   // in larger batches (like TransformPointList).
   {
      // points are to be different
      ASSERT(p != pResult);

      m3dVectComponent(pResult,0) = m3dVectComponent(p,0) * m[0][0] + m3dVectComponent(p,1) * m[1][0] + m3dVectComponent(p,2) * m[2][0] + m[3][0];
      m3dVectComponent(pResult,1) = m3dVectComponent(p,0) * m[0][1] + m3dVectComponent(p,1) * m[1][1] + m3dVectComponent(p,2) * m[2][1] + m[3][1];
      m3dVectComponent(pResult,2) = m3dVectComponent(p,0) * m[0][2] + m3dVectComponent(p,1) * m[1][2] + m3dVectComponent(p,2) * m[2][2] + m[3][2];
   }
#endif
}

/**************************************************************
* m3dMATR::_TransformPoint()
*
**************************************************************/
__INLINE void __CDECL m3dMATR::TransformPoint (m3dV * __RESTRICT p) const
{
   m3dCheckValid(p);

#if defined _AP_XENON && !defined M3D_SUPPRESS_OPTIMIZATION
   {
      XMMATRIX mtx    = XMLoadFloat4x4((const XMFLOAT4X4*)this->elements);
      XMVECTOR v      = XMLoadVector3(p);
      XMVECTOR result = XMVector3Transform(v, mtx);
      XMStoreVector3(p, result);
   }
   return;

#elif defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   {
	   vec_float4 m0 = vec_load_unaligned(this->elements, 0);
	   vec_float4 m1 = vec_load_unaligned(this->elements, 16);
	   vec_float4 m2 = vec_load_unaligned(this->elements, 32);
	   vec_float4 m3 = vec_load_unaligned(this->elements, 48);
	   vec_float4 v = vec_load_unaligned((float*)p);
	   vec_float4 xxxx = vec_splat(v, 0);
	   vec_float4 yyyy = vec_splat(v, 1);
	   vec_float4 zzzz = vec_splat(v, 2);

	   vec_float4 result = vec_madd(zzzz, m2, m3);
	   result = vec_madd(yyyy, m1, result);
	   result = vec_madd(xxxx, m0, result);

	   vec_store3_unaligned(result, (float*)p);
   }
#else
   int   i, j;
   m3dV  vResult;
   
   for (i = 0; i < 3; i++ )  {
      m3dVectComponent(&vResult, i) = 0.f;
      for (j = 0; j < 3; j++ )  {
         m3dVectComponent(&vResult,i) += m3dVectComponent(p,j) * m[j][i];
      }
      m3dVectComponent(&vResult,i) += m[j][i];
   }
   *p = vResult;
#endif
}

/**************************************************************
* m3dMATR::TransformVector()
*
**************************************************************/
__INLINE void __CDECL m3dMATR::TransformVector (const m3dV *p, m3dV *pResult) const
{
   m3dCheckValid(p);

   int     i, j;
   // vectors are to be different
   ASSERT(p != pResult);
   
   for (i = 0; i < 3; i++)  {
      m3dVectComponent(pResult,i) = 0.f;
      for (j = 0; j < 3; j++ )  {
         m3dVectComponent(pResult,i) += m3dVectComponent(p,j) * m[j][i];
      }
   }
}

/**************************************************************
* m3dMATR::_TransformVector()
*
**************************************************************/
__INLINE void __CDECL m3dMATR::TransformVector (m3dV * __RESTRICT p) const
{

   m3dCheckValid(p);

#if defined _AP_XENON && !defined M3D_SUPPRESS_OPTIMIZATION
   {
      XMMATRIX mtx    = XMLoadFloat4x4((const XMFLOAT4X4*)this->elements);
      XMVECTOR v      = XMLoadVector3(p);
      XMVECTOR result = XMVector3TransformNormal(v, mtx);
      XMStoreVector3(p, result);
   }
   return;

#elif defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   {
	   vec_float4 m0 = vec_load_unaligned(this->elements, 0);
	   vec_float4 m1 = vec_load_unaligned(this->elements, 16);
	   vec_float4 m2 = vec_load_unaligned(this->elements, 32);

	   vec_float4 v = vec_load_unaligned((float*)p);
	   vec_float4 xxxx = vec_splat(v, 0);
	   vec_float4 yyyy = vec_splat(v, 1);
	   vec_float4 zzzz = vec_splat(v, 2);

	   vec_float4 result = vec_madd(zzzz, m2, (vec_float4)(0.0f));
	   result = vec_madd(yyyy, m1, result);
	   result = vec_madd(xxxx, m0, result);

	   vec_store3_unaligned(result, (float*)p);
   }

#else //FPU version
   int   i, j;
   m3dV  vResult;
   
   for (i = 0; i < 3; i++ )  {
      m3dVectComponent(&vResult,i) = 0.f;
      for (j = 0; j < 3; j++ )  {
         m3dVectComponent(&vResult,i) += m3dVectComponent(p,j) * m[j][i];
      }
   }
   *p = vResult;
#endif
}

#if defined _AP_XENON
#include <xboxmath.h>
#endif
/*****************************************************************************
* m3dMATR::TransformNormal ()
*    Tranforms normal by matrix
*****************************************************************************/
void m3dMATR::TransformNormal (const m3dV *pNorm, m3dV *pRes) const
{
   m3dCheckValid(pNorm);

   ASSERT(m3dFAbs(m3dLengthVector_2(pNorm) - 1.f) < M3D_EPSILON);

//#if defined _AP_XENON
//
//	XMVECTOR N = XMLoadVector3(pNorm);
//   XMMATRIX M(this->elements);
//	XMVECTOR UNITX = __vpermwi(__vupkd3d(XMVectorZero(), VPACK_NORMSHORT2), 0xEA);
//   XMVECTOR T1 = XMVector3Cross(N, UNITX);
//   if (XMVector3Equal(T1, XMVectorZero()))
//	{
//		XMVECTOR UNITY = __vpermwi(__vupkd3d(XMVectorZero(), VPACK_NORMSHORT2), 0xBA);
//      T1 = XMVector3Cross(N, UNITY);
//	}
//	XMVECTOR T2 = XMVector3Cross(N,  T1);
//
//   T1 = XMVector3TransformNormal(T1, M);
//   T2 = XMVector3TransformNormal(T2, M);
//
//   XMVECTOR R = XMVector3Cross(T1, T2);
//	
//	ASSERT(!XMVector3Equal(R, XMVectorZero()));
//
//	R = XMVector3Normalize(R);
//	XMStoreVector3(pRes, R);
//
//#else

   m3dV  tang1, tang2;  // vectors in plane defined by normal

   // define plane by normal
   if (m3dFAbs(m3dDotProduct(pNorm, &m3dVUnitX)) > 0.5f) {
	   m3dCrossProduct(pNorm, &m3dVUnitY, &tang1);
   } else {
	   m3dCrossProduct(pNorm, &m3dVUnitX, &tang1);
   }

   m3dCrossProduct(pNorm, &tang1, &tang2);

   // transform plane
   TransformVector(&tang1);
   TransformVector(&tang2);

   // restore normal
   m3dCrossProduct(&tang1, &tang2, pRes);

   if (!_m3dNormalize(pRes)) {
      ASSERT(FALSE);
   }

//#endif

   m3dCheckValid(pRes);
   return;
}
/*****************************************************************************
* m3dMATR::TransformNormal ()
*    Tranforms normal by matrix
*****************************************************************************/
void m3dMATR::TransformNormal (m3dV *pNorm) const
{
   m3dV  norm;

   TransformNormal(pNorm, &norm);
   *pNorm = norm;
   return;
}
/**************************************************************
* m3dMATR::TransformPoly()
*
**************************************************************/
__INLINE void m3dMATR::TransformPoly (m3dPOLY *poly, m3dPOLY *polyResult) const
{
   int i;

   // polygons are to be different
   ASSERT(poly != polyResult);

   for (i = 0; i < m3dPolyNVert(poly); i++ )  {
      TransformPoint(m3dPolyVertex(poly, i), m3dPolyVertex(polyResult, i));
   }

   m3dPolyNVert(polyResult) = m3dPolyNVert(poly);

   return;
}

/**************************************************************
* m3dMATR::_TransformPoly()
*
**************************************************************/
__INLINE void m3dMATR::TransformPoly (m3dPOLY *poly) const
{
   int     i;

   for (i = 0; i < m3dPolyNVert(poly); i++)  {
      TransformPoint(m3dPolyVertex(poly,i));
   }
   return;
}


//
// 4D Transformations
//
/**************************************************************
* m3dMATR::TransformPoint()
*
**************************************************************/
__INLINE void m3dMATR::TransformPoint (m3dV const *p, m4dV *pResult) const
{
   m3dCheckValid(p);

   pResult->x = p->x * _11 + p->y * _21 + p->z * _31 + _41;
   pResult->y = p->x * _12 + p->y * _22 + p->z * _32 + _42;
   pResult->z = p->x * _13 + p->y * _23 + p->z * _33 + _43;
   pResult->w = p->x * _14 + p->y * _24 + p->z * _34 + _44;
}


__INLINE void __CDECL m3dMATR::TransformPointHomogeneous (m4dV const*p, m4dV *pResult) const
{
   m3dCheckValid(p);

   pResult->x = p->x * _11  +  p->y * _21  + p->z * _31  + p->w * _41;
   pResult->y = p->x * _12  +  p->y * _22  + p->z * _32  + p->w * _42;
   pResult->z = p->x * _13  +  p->y * _23  + p->z * _33  + p->w * _43;
   pResult->w = p->x * _14  +  p->y * _24  + p->z * _34  + p->w * _44;
}

/*************************************************************
'* m3dMATR::ReverseTransformPoint()
'* 
'* this allows to avoid inversion of a matrix:
'* suppose  m.TransformPoint       (x, y);      // y is result
'* then     m.ReverseTransformPoint(y, z);      // z == x (approximately)
'*************************************************************/
__INLINE void __CDECL m3dMATR::ReverseTransformPoint(m3dV const *p, m3dV *pResult) const
{
   m3dCheckValid(p);
   
   // points are to be different
   ASSERT(p != pResult);

   m3dV orgLCSinWCS = m3dVZero;
   orgLCSinWCS.x = -_41 * _11 - _42 * _12 - _43 * _13;
   orgLCSinWCS.y = -_41 * _21 - _42 * _22 - _43 * _23;
   orgLCSinWCS.z = -_41 * _31 - _42 * _32 - _43 * _33;

   m3dV reversedPwithoutShift = m3dVZero;
   reversedPwithoutShift.x = p->x * _11 + p->y * _12 + p->z * _13;
   reversedPwithoutShift.y = p->x * _21 + p->y * _22 + p->z * _23;
   reversedPwithoutShift.z = p->x * _31 + p->y * _32 + p->z * _33;

   *pResult = orgLCSinWCS + reversedPwithoutShift;
}

//
// 2D Transformations
//
/**************************************************************
* m2dMATR::TransformPoint ()
**************************************************************/
__INLINE void m2dMATR::TransformPoint (m2dV *p, m2dV *pResult) const
{
   int     i, j;

   m3dCheckValid(p);

   // points are to be different
   ASSERT(p != pResult);

   for (i = 0; i < 2; i++ )  {
      m2dVectComponent(pResult,i) = 0.f;
      for (j = 0; j < 2; j++ )  {
         m2dVectComponent(pResult,i) += m2dVectComponent(p,j) * m[j][i];
      }
      m2dVectComponent(pResult,i) += m[j][i];
   }

   return;
}
/**************************************************************
* m2dMATR::_TransformPoint ()
**************************************************************/
__INLINE void m2dMATR::TransformPoint (m2dV *p) const
{
   int   i, j;
   m2dV  vResult;

   m3dCheckValid(p);

   for (i = 0; i < 2; i++ )  {
      m2dVectComponent(&vResult,i) = 0.f;
      for (j = 0; j < 2; j++ )  {
         m2dVectComponent(&vResult,i) += m2dVectComponent(p,j) * m[j][i];
      }
      m2dVectComponent(&vResult,i) += m[j][i];
   }
   *p = vResult;

   return;
}
/**************************************************************
* m2dMATR::TransformVector ()
**************************************************************/
__INLINE void m2dMATR::TransformVector (m2dV *p, m2dV *pResult) const
{
   int     i, j;

   for (i = 0; i < 2; i++ )  {
      m2dVectComponent(pResult,i) = 0.f;
      for (j = 0; j < 2; j++ )  {
         m2dVectComponent(pResult,i) += m2dVectComponent(p,j) * m[j][i];
      }
   }
   return;
}
/**************************************************************
* m2dMATR::_TransformVector ()
**************************************************************/
__INLINE void m2dMATR::TransformVector (m2dV *p) const
{
   int     i, j;
   m2dV   vResult;

   m3dCheckValid(p);

   for (i = 0; i < 2; i++ )  {
      m2dVectComponent(&vResult,i) = 0.f;
      for (j = 0; j < 2; j++ )  {
         m2dVectComponent(&vResult,i) += m2dVectComponent(p,j) * m[j][i];
      }
   }
   *p = vResult;
   return;
}

/**************************************************************
* m2dMATR::TransformPoly ()
**************************************************************/
__INLINE void m2dMATR::TransformPoly (m2dPOLY *poly, m2dPOLY *polyResult) const
{
   int i;

   for (i = 0; i < m3dPolyNVert(poly); i++ )  {
      TransformPoint(m3dPolyVertex(poly,i), m3dPolyVertex(polyResult,i));
   }
   m3dPolyNVert(polyResult) = m3dPolyNVert(poly);
   return;
}


extern m3dMATR m3dMatrIdentity;
extern m2dMATR m2dMatrIdentity;

//
// End of file 'M3D_MATR.H'
//
